#ifndef ATTITUDESOLUTION_H
#define ATTITUDESOLUTION_H

#include "mpu6500.h"
#include "math.h"
#include "bsp_math.h"
#define Kp 1.5f
#define Ki 0.005f
#define halfT (dt / 2)

#define q0 (mpu_data->q[0])
#define q1 (mpu_data->q[1])
#define q2 (mpu_data->q[2])
#define q3 (mpu_data->q[3])

#define q0q0 (q0 * q0)
#define q0q1 (q0 * q1)
#define q0q2 (q0 * q2)
#define q0q3 (q0 * q3)
#define q1q1 (q1 * q1)
#define q1q2 (q1 * q2)
#define q1q3 (q1 * q3)
#define q2q2 (q2 * q2)
#define q2q3 (q2 * q3)
#define q3q3 (q3 * q3)
#define exInt (mpu_data->exint)
#define eyInt (mpu_data->eyint)
#define ezInt (mpu_data->ezint)

void Mahony(Mpu6500_DataTydef *, float, float, float, float, float, float);

#endif
